#!/usr/bin/env python3
# ---------- client_node.py -------------
# 改进版多机器人客户端节点
# 集成 move_base 导航系统，修复通信协议问题

import rospy
import socket
import struct
import time
import zlib
import threading
import tf
import actionlib
import numpy as np
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from nav_msgs.msg import Odometry
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionResult
from actionlib_msgs.msg import GoalStatusArray, GoalID
from std_msgs.msg import Header

# 通信协议常量定义
FRAME_SIZE = 32  # 固定帧大小
VERSION = 0x02  # 协议版本

# 指令类型
COMMAND_TYPES = {
    "ConnectionRequest": 0x01,  # 连接请求 (客户端→服务端)
    "ConnectionResponse": 0x02,  # 连接响应 (服务端→客户端)
    "PositionReport": 0x03,  # 位置报告 (客户端→服务端)
    "NavigationGoal": 0x04,  # 导航目标 (服务端→客户端)
    "Heartbeat": 0x05,  # 心跳包 (双向)
    "CommandAck": 0x06,  # 指令确认 (双向)
    "EmergencyStop": 0x07,  # 紧急停止 (服务端→客户端)
    "StatusReport": 0x08,  # 状态报告 (客户端→服务端)
    "RoleAssignment": 0x09,
}

# 状态码定义
STATUS_CODES = {
    "Success": 0x00,
    "GeneralError": 0x01,
    "InvalidCommand": 0x02,
    "CRCError": 0x03,
    "Timeout": 0x04,
    "RobotBusy": 0x05,
}

# move_base 状态映射
MOVE_BASE_STATUS = {
    0: "PENDING",  # 目标待处理
    1: "ACTIVE",  # 目标正在执行
    2: "PREEMPTED",  # 目标被抢占
    3: "SUCCEEDED",  # 目标成功完成
    4: "ABORTED",  # 目标中止
    5: "REJECTED",  # 目标被拒绝
    6: "PREEMPTING",  # 目标正在被抢占
    7: "RECALLING",  # 目标正在被召回
    8: "RECALLED",  # 目标被召回
    9: "LOST",  # 目标丢失
}


class TCPClient:
    """TCP通信客户端类，封装所有底层通信功能"""

    def __init__(self, server_ip, server_port, robot_name, frame_handler):
        self.server_ip = server_ip
        self.server_port = server_port
        self.robot_name = robot_name
        self.frame_handler = frame_handler  # 帧处理回调函数

        # 连接状态
        self.connected = False
        self.connection_established = False
        self.robot_id = "robot0"

        # 通信统计
        self.packets_sent = 0
        self.packets_received = 0
        self.crc_errors = 0
        self.debug_mode = rospy.get_param("~debug", False)

        # 心跳管理
        self.last_heartbeat_time = 0
        self.heartbeat_interval = 2.0

        # 线程管理
        self.connect_thread = None
        self.receive_thread = None
        self.running = False

        # 启动连接线程
        self.start()

    def start(self):
        """启动TCP客户端"""
        self.running = True
        self.connect_thread = threading.Thread(
            target=self.connect_to_server, daemon=True
        )
        self.connect_thread.start()
        rospy.loginfo(
            f"[{self.robot_name}] TCP客户端启动，尝试连接服务器 {self.server_ip}:{self.server_port}"
        )

    def stop(self):
        """停止TCP客户端"""
        self.running = False
        self.connected = False
        if hasattr(self, "sock") and self.sock:
            try:
                self.sock.close()
            except:
                pass
        rospy.loginfo(f"[{self.robot_name}] TCP客户端已停止")

    def build_frame(self, command, payload=None, robot_id=None):
        """构建通信帧"""
        if robot_id is None:
            robot_id = self.robot_id

        # 获取当前时间戳(毫秒)
        timestamp = int(time.time() * 1000)

        # 处理负载
        if payload is None:
            payload = bytes(14)
        elif len(payload) > 14:
            payload = payload[:14]
        elif len(payload) < 14:
            payload = payload.ljust(14, b"\x00")

        # 构建帧头部分
        header = struct.pack(">BBiQ", VERSION, command, robot_id, timestamp)
        frame_data = header + payload

        # 计算CRC32校验码
        crc32 = zlib.crc32(frame_data) & 0xFFFFFFFF
        crc_bytes = struct.pack(">I", crc32)

        full_frame = frame_data + crc_bytes
        return full_frame

    def parse_frame(self, data):
        """解析通信帧"""
        if len(data) != FRAME_SIZE:
            rospy.logerr(f"[{self.robot_name}] 无效帧长度: {len(data)}字节 (预期{FRAME_SIZE}字节)")
            return None

        # 分离数据和校验码
        frame_data = data[:28]
        received_crc = struct.unpack(">I", data[28:32])[0]

        # 验证CRC
        calculated_crc = zlib.crc32(frame_data) & 0xFFFFFFFF
        if received_crc != calculated_crc:
            self.crc_errors += 1
            rospy.logwarn(
                f"[{self.robot_name}] CRC校验失败! 接收: {received_crc:08X}, 计算: {calculated_crc:08X}"
            )
            return None

        # 解析帧头
        try:
            header_part = frame_data[:14]
            version, command, robot_id, timestamp = struct.unpack(">BBiQ", header_part)
            payload = frame_data[14:28]

            # 验证协议版本
            if version != VERSION:
                rospy.logwarn(f"[{self.robot_name}] 协议版本不匹配! 接收: {version}, 预期: {VERSION}")
                return None

            return {
                "version": version,
                "command": command,
                "robot_id": robot_id,
                "timestamp": timestamp,
                "payload": payload,
            }

        except struct.error as e:
            rospy.logerr(f"[{self.robot_name}] 帧解析错误: {e}")
            return None

    def connect_to_server(self):
        """连接到服务器并完成握手协议"""
        while self.running and not self.connection_established:
            try:
                # 创建新的socket连接
                if hasattr(self, "sock"):
                    self.sock.close()
                self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                self.sock.settimeout(10.0)
                rospy.loginfo(f"[{self.robot_name}] 尝试连接到 {self.server_ip}:{self.server_port}...")
                self.sock.connect((self.server_ip, self.server_port))
                self.connected = True
                rospy.loginfo(f"[{self.robot_name}] TCP连接已建立")

                # 发送连接请求
                name_bytes = self.robot_name.encode("utf-8")
                if len(name_bytes) > 14:
                    name_bytes = name_bytes[:14]
                elif len(name_bytes) < 14:
                    name_bytes = name_bytes.ljust(14, b"\0")

                frame = self.build_frame(
                    command=COMMAND_TYPES["ConnectionRequest"],
                    payload=name_bytes,
                    robot_id=0,
                )

                self.sock.sendall(frame)
                self.packets_sent += 1
                rospy.loginfo(f"[{self.robot_name}] 已发送连接请求")

                # 等待连接响应
                response = self.sock.recv(FRAME_SIZE)
                if not response:
                    rospy.logwarn(f"[{self.robot_name}] 服务器未响应，重新连接...")
                    self.sock.close()
                    time.sleep(2)
                    continue

                # 解析响应
                frame_data = self.parse_frame(response)
                if not frame_data:
                    rospy.logerr(f"[{self.robot_name}] 连接响应解析失败")
                    self.sock.close()
                    time.sleep(2)
                    continue

                # 检查是否为连接响应
                if frame_data["command"] != COMMAND_TYPES["ConnectionResponse"]:
                    rospy.logwarn(f"[{self.robot_name}] 收到非预期命令: {frame_data['command']}")
                    self.sock.close()
                    time.sleep(2)
                    continue

                # 获取分配的机器人ID
                self.robot_id = struct.unpack(">i", frame_data["payload"][:4])[0]
                rospy.loginfo(f"[{self.robot_name}] 服务器分配机器人ID: {self.robot_id}")

                # 发送确认
                ack_payload = struct.pack(
                    ">BB", COMMAND_TYPES["ConnectionResponse"], STATUS_CODES["Success"]
                )
                ack_frame = self.build_frame(
                    command=COMMAND_TYPES["CommandAck"], payload=ack_payload
                )
                self.sock.sendall(ack_frame)
                self.packets_sent += 1

                # 标记连接已建立
                self.connection_established = True
                self.last_heartbeat_time = time.time()

                # 启动接收线程
                if not self.receive_thread or not self.receive_thread.is_alive():
                    self.receive_thread = threading.Thread(
                        target=self.receive_loop, daemon=True
                    )
                    self.receive_thread.start()

                rospy.loginfo(f"[{self.robot_name}] 连接协议完成，准备开始通信")

            except (socket.timeout, ConnectionRefusedError) as e:
                rospy.logwarn(f"[{self.robot_name}] 连接失败: {e}, 重试...")
                time.sleep(2)
            except (socket.error, OSError) as e:
                rospy.logerr(f"[{self.robot_name}] 连接错误: {e}")
                self.connected = False
                time.sleep(5)
            except Exception as e:
                rospy.logerr(f"[{self.robot_name}] 连接过程中发生意外错误: {e}")
                self.connected = False
                time.sleep(5)

    def receive_loop(self):
        """接收数据的循环"""
        rospy.loginfo(f"[{self.robot_name}] 启动接收循环")
        self.sock.settimeout(8.0)

        while self.running and self.connected:
            try:
                # 接收完整帧
                data = b""
                while len(data) < FRAME_SIZE:
                    chunk = self.sock.recv(FRAME_SIZE - len(data))
                    if not chunk:
                        rospy.logwarn(f"[{self.robot_name}] 连接被服务器关闭")
                        self.connected = False
                        break
                    data += chunk

                if not self.connected or not data:
                    break

                self.packets_received += 1

                # 解析帧
                frame = self.parse_frame(data)
                if frame:
                    # 调用帧处理回调
                    self.frame_handler(frame)

            except socket.timeout:
                # 检查心跳
                current_time = time.time()
                if (
                    current_time - self.last_heartbeat_time
                    > self.heartbeat_interval * 3
                ):
                    rospy.logwarn(f"[{self.robot_name}] 心跳超时，尝试重新连接...")
                    self.connected = False
                    self.connection_established = False
                    break
                else:
                    # 发送心跳包
                    self.send_heartbeat()
            except (socket.error, OSError) as e:
                rospy.logerr(f"[{self.robot_name}] 接收数据错误: {e}")
                self.connected = False
                break
            except Exception as e:
                rospy.logerr(f"[{self.robot_name}] 接收循环中发生意外错误: {e}")
                self.connected = False
                break

        rospy.loginfo(f"[{self.robot_name}] 接收循环结束")
        # 尝试重新连接
        if self.running and not self.connection_established:
            rospy.loginfo(f"[{self.robot_name}] 尝试重新连接服务器...")
            if not self.connect_thread or not self.connect_thread.is_alive():
                self.connect_thread = threading.Thread(
                    target=self.connect_to_server, daemon=True
                )
                self.connect_thread.start()

    def send_frame(self, command, payload=None, robot_id=None):
        """发送帧到服务器"""
        if not self.connection_established:
            rospy.logwarn(f"[{self.robot_name}] 未连接服务器，无法发送帧")
            return False

        try:
            frame = self.build_frame(command, payload, robot_id)
            self.sock.sendall(frame)
            self.packets_sent += 1
            if self.debug_mode:
                rospy.logdebug(f"[{self.robot_name}] 发送帧: command={command}, payload_len={len(payload) if payload else 0}")
            return True
        except (socket.error, OSError) as e:
            rospy.logerr(f"[{self.robot_name}] 发送数据失败: {e}")
            self.connected = False
            return False

    def send_heartbeat(self):
        """发送心跳包"""
        if not self.connection_established:
            return

        try:
            # 使用当前时间戳作为序列号
            seq = int(time.time() * 1000) % 1000000
            payload = struct.pack(">i", seq)

            self.send_frame(COMMAND_TYPES["Heartbeat"], payload)
            if self.debug_mode:
                rospy.logdebug(f"[{self.robot_name}] 发送心跳包, seq={seq}")
        except Exception as e:
            rospy.logerr(f"[{self.robot_name}] 发送心跳包错误: {e}")


class EnhancedRobotClient:
    def __init__(self, robot_name, server_ip, server_port):
        self.robot_name = robot_name
        self.server_ip = server_ip
        self.server_port = server_port

        # ROS 命名空间处理
        self.namespace = rospy.get_namespace().rstrip("/")
        if self.namespace:
            rospy.loginfo(f"[{self.robot_name}] 使用命名空间: {self.namespace}")

        # ROS 通信设置
        self.map_frame_id = rospy.get_param("~map_frame", "map")
        self.base_frame_id = rospy.get_param("~base_frame", "base_link")
        self.goal_topic = rospy.get_param("~goal_topic", "move_base_simple/goal")
        self.odom_topic = rospy.get_param("~odom_topic", "odom")
        self.cancel_topic = rospy.get_param("~cancel_topic", "move_base/cancel")
        self.status_topic = rospy.get_param("~status_topic", "move_base/status")

        # 初始化发布器和监听器
        self.goal_pub = rospy.Publisher(self.goal_topic, PoseStamped, queue_size=10)
        self.cancel_pub = rospy.Publisher(self.cancel_topic, GoalID, queue_size=10)
        self.tf_listener = tf.TransformListener()

        # 导航状态跟踪
        self.current_goal_id = None
        self.navigation_status = 0  # 0:空闲, 1:移动中, 2:到达目标, 3:错误
        self.last_goal_time = rospy.Time(0)

        # 创建TCP客户端
        self.tcp_client = TCPClient(
            server_ip, server_port, robot_name, self.handle_frame  # 注册帧处理回调
        )

        # 启动状态报告定时器
        self.status_timer = rospy.Timer(
            rospy.Duration(2.0), self.status_report_callback
        )

        # 订阅 move_base 状态
        rospy.Subscriber(self.status_topic, GoalStatusArray, self.nav_status_callback)

        rospy.loginfo(f"[{self.robot_name}] 机器人客户端初始化完成")

    def handle_frame(self, frame):
        """处理接收到的通信帧"""
        # 检查机器人ID是否匹配
        if frame["robot_id"] != 0 and frame["robot_id"] != self.tcp_client.robot_id:
            if self.tcp_client.debug_mode:
                rospy.logdebug(
                    f"[{self.robot_name}] 收到非本机ID的帧: {frame['robot_id']} (本机ID: {self.tcp_client.robot_id})"
                )
            return

        # 处理不同命令
        if frame["command"] == COMMAND_TYPES["NavigationGoal"]:
            self.handle_navigation_command(frame["payload"])
        elif frame["command"] == COMMAND_TYPES["Heartbeat"]:
            self.handle_heartbeat(frame["payload"])
        elif frame["command"] == COMMAND_TYPES["EmergencyStop"]:
            self.handle_emergency_stop()
        elif frame["command"] == COMMAND_TYPES["CommandAck"]:
            self.handle_command_ack(frame["payload"])
        elif frame["command"] == COMMAND_TYPES["RoleAssignment"]:
            self.handle_role_assignment(frame["payload"])
        else:
            rospy.logwarn(f"[{self.robot_name}] 收到未知命令: {frame['command']}")

    def handle_navigation_command(self, payload):
        """处理导航目标指令"""
        try:
            if len(payload) >= 12:  # 3个float(4*3=12字节)
                x, y, timeout = struct.unpack(">fff", payload[:12])
                priority = 1  # 默认优先级
                rospy.loginfo(
                    f"[{self.robot_name}] 收到导航目标: ({x:.2f}, {y:.2f}), 超时: {timeout:.1f}秒, 优先级: {priority}"
                )
            else:
                rospy.logerr(f"[{self.robot_name}] 导航目标负载长度不足: {len(payload)}字节")
                return

            start_time = time.time()
            while self.goal_pub.get_num_connections() == 0:
                if time.time() - start_time > 10.0:
                    rospy.logerr(f"[{self.robot_name}] 无法连接到move_base，目标发布失败")
                    return
                rospy.sleep(0.2)
            rospy.loginfo(
                f"[{self.robot_name}] 检查到存在 {self.goal_pub.get_num_connections()} 个订阅者，准备发布目标"
            )
            # 创建并发布目标点
            goal = PoseStamped()
            goal.header.stamp = rospy.Time.now()
            goal.header.frame_id = self.map_frame_id
            goal.pose.position.x = x
            goal.pose.position.y = y

            # 获取当前位置并计算方向
            current_pos = self.get_current_position()
            if current_pos:
                curr_x, curr_y, _ = current_pos
                dx = x - curr_x
                dy = y - curr_y
                theta = np.arctan2(dy, dx)
                quat = tf.transformations.quaternion_from_euler(0.0, 0.0, theta)
                goal.pose.orientation.x = quat[0]
                goal.pose.orientation.y = quat[1]
                goal.pose.orientation.z = quat[2]
                goal.pose.orientation.w = quat[3]
                rospy.logdebug(f"[{self.robot_name}] 计算目标朝向: theta={theta:.2f} rad")
            else:
                # 回退到默认朝向
                goal.pose.orientation.w = 1.0
                rospy.logwarn(f"[{self.robot_name}] 无法获取当前位置，使用默认朝向")

            self.goal_pub.publish(goal)
            self.last_goal_time = rospy.Time.now()
            rospy.loginfo(f"[{self.robot_name}] 已发布导航目标到 {self.goal_topic}")

            # 发送确认
            ack_payload = struct.pack(
                ">BB", COMMAND_TYPES["NavigationGoal"], STATUS_CODES["Success"]
            )
            self.tcp_client.send_frame(
                command=COMMAND_TYPES["CommandAck"], payload=ack_payload
            )

        except Exception as e:
            rospy.logerr(f"[{self.robot_name}] 处理导航目标错误: {e}")
            # 发送错误确认
            ack_payload = struct.pack(
                ">BB", COMMAND_TYPES["NavigationGoal"], STATUS_CODES["GeneralError"]
            )
            self.tcp_client.send_frame(
                command=COMMAND_TYPES["CommandAck"], payload=ack_payload
            )

    def handle_heartbeat(self, payload):
        """处理心跳包"""
        try:
            # 解析心跳序列号
            if len(payload) >= 4:
                seq = struct.unpack(">i", payload[:4])[0]
                if self.tcp_client.debug_mode:
                    rospy.logdebug(f"[{self.robot_name}] 收到心跳包, 序列号: {seq}")
            else:
                seq = 0
                rospy.logwarn(f"[{self.robot_name}] 心跳包负载长度不足: {len(payload)}字节")

            # 更新最后心跳时间
            self.tcp_client.last_heartbeat_time = time.time()

            # 发送心跳响应
            heartbeat_payload = struct.pack(">i", seq)
            self.tcp_client.send_frame(
                command=COMMAND_TYPES["Heartbeat"], payload=heartbeat_payload
            )

        except Exception as e:
            rospy.logerr(f"[{self.robot_name}] 处理心跳包错误: {e}")

    def handle_emergency_stop(self):
        """处理紧急停止指令 - 停止所有导航目标"""
        rospy.logwarn(f"[{self.robot_name}] 收到紧急停止指令! 正在取消所有导航目标...")

        try:
            # 创建取消目标消息
            cancel_msg = GoalID()
            self.cancel_pub.publish(cancel_msg)
            rospy.loginfo(f"[{self.robot_name}] 已发布取消命令到 move_base")

            # 更新导航状态
            self.navigation_status = 0  # 空闲状态

            # 发送确认
            ack_payload = struct.pack(
                ">BB", COMMAND_TYPES["EmergencyStop"], STATUS_CODES["Success"]
            )
            self.tcp_client.send_frame(
                command=COMMAND_TYPES["CommandAck"], payload=ack_payload
            )

        except Exception as e:
            rospy.logerr(f"[{self.robot_name}] 处理紧急停止错误: {e}")
            # 发送错误确认
            ack_payload = struct.pack(
                ">BB", COMMAND_TYPES["EmergencyStop"], STATUS_CODES["GeneralError"]
            )
            self.tcp_client.send_frame(
                command=COMMAND_TYPES["CommandAck"], payload=ack_payload
            )

    def handle_command_ack(self, payload):
        """处理指令确认"""
        try:
            if len(payload) >= 2:
                acked_command, status_code = struct.unpack(">BB", payload[:2])
                status_msg = next(
                    (k for k, v in STATUS_CODES.items() if v == status_code), "Unknown"
                )
                if self.tcp_client.debug_mode:
                    rospy.logdebug(
                        f"[{self.robot_name}] 收到指令确认: 命令={acked_command}, 状态={status_msg}({status_code})"
                    )
            else:
                rospy.logwarn(f"[{self.robot_name}] 指令确认负载长度不足: {len(payload)}字节")
        except Exception as e:
            rospy.logwarn(f"[{self.robot_name}] 解析指令确认错误: {e}")

    def nav_status_callback(self, status_msg):
        """move_base 状态回调函数"""
        if not status_msg.status_list:
            # 没有活动目标
            self.navigation_status = 0  # 空闲
            return

        # 获取最新目标状态
        latest_status = status_msg.status_list[-1]

        # 记录当前目标ID
        self.current_goal_id = latest_status.goal_id.id

        # 映射导航状态
        if latest_status.status == 1:  # ACTIVE
            self.navigation_status = 1  # 移动中
        elif latest_status.status == 3:  # SUCCEEDED
            self.navigation_status = 2  # 到达目标
        elif latest_status.status in [4, 5, 9]:  # ABORTED, REJECTED, LOST
            self.navigation_status = 3  # 错误状态
        else:
            self.navigation_status = 1  # 其他状态视为移动中

        if self.tcp_client.debug_mode:
            status_name = MOVE_BASE_STATUS.get(latest_status.status, "UNKNOWN")
            rospy.logdebug(f"[{self.robot_name}] move_base状态更新: {status_name} ({latest_status.status})")

    def handle_role_assignment(self, payload):
        """处理角色分配指令"""
        try:
            if len(payload) >= 1:
                role_code = payload[0]  # 第一个字节是角色代码
                if role_code == 0x01:
                    rospy.loginfo(f"[{self.robot_name}] 分配角色: 逃避者")
                elif role_code == 0x02:
                    rospy.loginfo(f"[{self.robot_name}] 分配角色: 追捕者")
                else:
                    rospy.logwarn(f"[{self.robot_name}] 未知角色代码: {role_code}")

                # 发送确认
                ack_payload = struct.pack(
                    ">BB", COMMAND_TYPES["RoleAssignment"], STATUS_CODES["Success"]
                )
                self.tcp_client.send_frame(
                    command=COMMAND_TYPES["CommandAck"], payload=ack_payload
                )

        except Exception as e:
            rospy.logerr(f"[{self.robot_name}] 处理角色分配错误: {e}")
            # 错误确认
            ack_payload = struct.pack(
                ">BB", COMMAND_TYPES["RoleAssignment"], STATUS_CODES["GeneralError"]
            )
            self.tcp_client.send_frame(
                command=COMMAND_TYPES["CommandAck"], payload=ack_payload
            )

    def status_report_callback(self, event):
        """定时报告机器人状态和位置"""
        if not self.tcp_client.connection_established:
            return

        try:
            # 获取当前位置
            position = self.get_current_position()
            if position is None:
                rospy.logwarn(f"[{self.robot_name}] 无法获取当前位置")
                return

            x, y, _ = position  # 舍弃朝向信息

            # 获取机器人状态
            status = self.get_robot_status()

            # 构建负载
            payload = struct.pack(">ffB", x, y, status)
            payload = payload.ljust(14, b"\x00")  # 填充到14字节

            # 发送帧
            self.tcp_client.send_frame(
                command=COMMAND_TYPES["PositionReport"], payload=payload
            )
            if self.tcp_client.debug_mode:
                rospy.logdebug(f"[{self.robot_name}] 发送位置报告: ({x:.2f}, {y:.2f}), 状态: {status}")

        except (
            tf.LookupException,
            tf.ConnectivityException,
            tf.ExtrapolationException,
        ) as e:
            rospy.logwarn(f"[{self.robot_name}] TF错误: {e}")
        except Exception as e:
            rospy.logerr(f"[{self.robot_name}] 位置报告错误: {e}")

    def get_current_position(self):
        """使用TF获取当前机器人在地图中的位置"""
        try:
            # 等待TF数据可用
            self.tf_listener.waitForTransform(
                self.map_frame_id,
                self.base_frame_id,
                rospy.Time(0),
                rospy.Duration(1.0),
            )

            # 获取当前位置
            (trans, rot) = self.tf_listener.lookupTransform(
                self.map_frame_id, self.base_frame_id, rospy.Time(0)
            )

            # 计算朝向角
            _, _, theta = tf.transformations.euler_from_quaternion(rot)

            return (trans[0], trans[1], theta)

        except (
            tf.LookupException,
            tf.ConnectivityException,
            tf.ExtrapolationException,
            tf.Exception,
        ) as e:
            rospy.logwarn(f"[{self.robot_name}] 获取位置失败: {e}")
            return None

    def get_robot_status(self):
        """获取机器人综合状态"""
        return self.navigation_status

    def run(self):
        """主运行循环"""
        rospy.spin()
        # 清理
        self.tcp_client.stop()


if __name__ == "__main__":
    rospy.init_node("multi_robot_client")

    # 获取参数
    robot_name = rospy.get_param("~robot_name", "robot1")
    server_ip = rospy.get_param("~server_ip", "127.0.0.1")
    server_port = rospy.get_param("~server_port", 6001)

    rospy.loginfo(f"启动机器人客户端: {robot_name}")

    client = EnhancedRobotClient(robot_name, server_ip, server_port)
    client.run()